/**
* @file Decoder.cpp in InsCubeDecoder
* @author rebeater
* @comment
* Create on 2/22/22 11:03 AM
* @version 1.0
**/


#include "Decoder.h"
#include "AdiHandle.h"
#include <iostream>
#include <string>
#include <fmt/format.h>
#include <glog/logging.h>
#include "RstHandle.h"
#include "boost/circular_buffer.hpp"

namespace InsCube {

template<>
void BaseDecoder<MIX, ADIHandleBase>::Load(const std::string &raw_path) {

  std::ifstream ifs{raw_path, std::ios::binary};
  ifs.seekg(0, std::ios::end);
  size_t size = ifs.tellg();
  ifs.seekg(0, std::ios::beg);
  LOG(INFO) << fmt::format("Loading... [file size = {:.4f} MB] \n", (double)size / 1024.0 / 1024.0);
  LOG(INFO) << fmt::format("IMU name: {}", handle.GetDeviceName());
  p_data = new RawDataDef[size / 64 + 1];
  auto *raw = static_cast<RawDataDef *>(p_data);
  while (!ifs.eof()) {
	ifs.read(reinterpret_cast<char *>(raw), 64);
	raw++;
  }
  data_cnt = (uint32_t)(raw - (RawDataDef *)p_data) / sizeof(DataTypeDef) - 1;
  LOG(INFO) << "Load finished, data_cnt: " << data_cnt;
  LOG(INFO) << fmt::format("Time range form:{:.5f} to {:.5f}", ((RawDataDef *)p_data)->gpst, (raw - 2)->gpst);//
  // "Time range form:" <<  << " to:" << (raw - 2)->gpst;
}
template<>
void BaseDecoder<RST, RstHandle>::Load(const std::string &raw_path) {
  using RingBuffer = boost::circular_buffer<uint8_t>;
  RingBuffer ring_buffer{1024};
  std::ifstream ifs{raw_path, std::ios::binary};
  ifs.seekg(0, std::ios::end);
  size_t size = ifs.tellg();
  ifs.seekg(0, std::ios::beg);
  LOG(INFO) << fmt::format("Loading...{} [file size = {:.4f} MB] \n", raw_path,(double)size / 1024.0 / 1024.0);
  LOG(INFO) << fmt::format("Device name: {}", handle.GetDeviceName());
  p_data = new NavOutput[size / handle.frame_length + 1];
  LOG(INFO) << fmt::format("Approximate {:d} of frames", size / handle.frame_length);
  data_cnt = 0;
  auto *raw = static_cast<NavOutput *>(p_data);
  char buffer[32];
  NavOutput nav;
  uint8_t frame[handle.frame_length];
  while (!ifs.eof()) {
	ifs.read(buffer, 32);
	for (const auto &d:buffer) {
	  ring_buffer.push_back(d);
	}
	while (ring_buffer.size() >= handle.frame_length) {
	  if (ring_buffer.front() == 0xAA) {
		frame[0] = ring_buffer.front();
		ring_buffer.pop_front();
		if (ring_buffer.front() == 0x55) {
		  for (int i = 1; i < handle.frame_length; i++) {
			frame[i] = ring_buffer.front();
			ring_buffer.pop_front();
		  }
		  if (handle.ParserNav(frame, nav)) {
			raw[data_cnt] = nav;
			++data_cnt;
		  } else {
			LOG_EVERY_N(INFO, 10) << "check failed:";
		  };
		}
	  } else {
		ring_buffer.pop_front();
	  }
	}
  }/*end of read*/
  LOG(INFO) << "Load finished, data_cnt: " << data_cnt;
  LOG_IF(INFO,data_cnt>0) << fmt::format("Time range form:{:.5f} to {:.5f}", ((NavOutput *)p_data)->gpst, (raw[data_cnt-1]).gpst);//
}

/*模板特化*/
template<>
template<>
void BaseDecoder<MIX, ADIHandleBase>::SaveTo<ImuTxt>(const std::string &outpath) {
  std::ofstream ofs(outpath);
  ImuData imu;
  auto *praw = (RawDataDef *)p_data;
  LOG(INFO) << "Save imu data to:" << outpath;
  for (int i = 0; i < data_cnt; i++) {
	if (praw[i].type_ == DATA_TYPE_IMU) {
	  if (praw[i].imu_.raw_.stat != 0) {
		LOG_FIRST_N(WARNING, 10) << fmt::format("IMU status error {} at {}", praw[i].imu_.raw_.stat, praw[i].gpst);
	  }
	  if (handle.CheckSum(praw[i].imu_.raw_)) {
		LOG_FIRST_N(WARNING, 10) << fmt::format("IMU check sum error {} at {}", praw[i].imu_.raw_.stat, praw->gpst);
	  }
	  handle.ConvertRaw2D(praw[i].imu_.raw_, imu);
	  imu.gpst = praw[i].gpst;
	  ofs << fmt::format("{:.4f} {:10.5f} {:10.5f} {:10.5f} {:10.5f} {:10.5f} {:10.5f}\n",
						 imu.gpst, imu.gyro[0], imu.gyro[1], imu.gyro[2],
						 imu.acce[0], imu.acce[1], imu.acce[2]
	  );
	}
  }
}
template<>
template<>
void BaseDecoder<MIX, ADIHandleBase>::SaveTo<Imd>(const std::string &outpath) {
  LOG(INFO) << fmt::format("Save IMU data to imd:{} at rate {}, current gravity = {:.5f}",
						   outpath,
						   handle.GetRate(),
						   handle.GetG());
  std::ofstream ofs{outpath, std::ios::binary};
  ImuData imu;
  auto *praw = static_cast<RawDataDef *>(p_data);
  for (int i = 0; i < data_cnt; i++) {
	if (praw[i].type_ == DATA_TYPE_IMU) {
	  if (praw[i].imu_.raw_.stat != 0) {
		LOG_FIRST_N(WARNING, 5) << fmt::format("IMU status error 0x{:x} at {}", praw[i].imu_.raw_.stat, praw[i].gpst);
	  }
	  if (handle.CheckSum(praw[i].imu_.raw_)) {
		LOG_FIRST_N(WARNING, 5) << fmt::format("IMU check sum error {} at {}", praw[i].imu_.raw_.stat, praw[i].gpst);
	  }
	  handle.ConvertRaw2D(praw[i].imu_.raw_, imu);
	  imu.gpst = praw[i].gpst;
	  handle.Absolute2Increment(imu, true);
	  ofs.write(reinterpret_cast<const char *>(&imu), sizeof imu);
	}
  }
}
template<>
template<>
void BaseDecoder<MIX, ADIHandleBase>::SaveTo<GnssTxt>(const std::string &outpath) {
  LOG(INFO) << "Save gnss data to:" << outpath;
  std::ofstream ofs{outpath, std::ios::binary};
  auto *praw = static_cast<RawDataDef *>(p_data);
  for (int i = 0; i < data_cnt; i++) {
	if (praw[i].type_ == DATA_TYPE_GNSS and praw[i].gnss_.week >= 0) {
	  ofs << fmt::format(
//		  "{:d} {:.5f} {:.12f} {:.12f} {:.5f} {:.3f} {:.3f} {:.3f} {:.2f} {:.2f} {:.2f} {:.2f} {:d} {:d}\n",
		  "{:d} {:.5f} {:.12f} {:.12f} {:.5f} {:.3f} {:.3f} {:.3f} {:d} {:d}\n",
		  praw[i].gnss_.week,
		  praw[i].gpst,
		  praw[i].gnss_.lat,
		  praw[i].gnss_.lon,
		  praw[i].gnss_.height,
		  praw[i].gnss_.pos_std[0],
		  praw[i].gnss_.pos_std[1],
		  praw[i].gnss_.pos_std[2],
/*		  praw[i].gnss_.yaw,
		  praw[i].gnss_.yaw_std_100 / 100.0,
		  praw[i].gnss_.pitch,
		  praw[i].gnss_.pitch_std_100 / 100.0,*/
		  praw[i].gnss_.ns,
		  praw[i].gnss_.mode
	  );
	}
	if (praw[i].gnss_.week <= 0) {
	  LOG_FIRST_N(WARNING, 10) << "Gnss is bad at " << praw[i].gpst;
	}
  }
}

template<>
template<>
void BaseDecoder<MIX, ADIHandleBase>::SaveTo<BmpTxt>(const std::string &outpath) {
  LOG(INFO) << "Save bmp280 data to:" << outpath;
  std::ofstream ofs{outpath, std::ios::binary};
  auto *praw = static_cast<RawDataDef *>(p_data);
  for (int i = 0; i < data_cnt; i++) {
	if (praw[i].type_ == DATA_TYPE_BMP) {
	  ofs << fmt::format(
		  "{:.5f} {:.4f} {:.4f}\n",
		  praw[i].gpst,
		  praw[i].bmp_.pressure,
		  praw[i].bmp_.temperature
	  );
	}
  }
}

template<>
template<>
void BaseDecoder<MIX, ADIHandleBase>::SaveTo<VelTxt>(const std::string &outpath) {
  LOG(INFO) << "Save odometer data to:" << outpath;
  std::ofstream ofs{outpath, std::ios::binary};
  auto *praw = static_cast<RawDataDef *>(p_data);
  for (int i = 0; i < data_cnt; i++) {
	if (praw[i].type_ == DATA_TYPE_VEL) {
	  double forward = ((float)((int16_t)(praw[i].vel_.vh_ << 8u) | praw[i].vel_.vl_)) / 1000.0f,
		  angular = ((float)((int16_t)(praw[i].vel_.ah_ << 8u) | praw[i].vel_.al_)) / 1000.0f;
	  ofs << fmt::format(
		  "{:.5f} {:.4} {:.4}\n",
		  praw[i].gpst,
		  forward,
		  angular
	  );
	}
  }
}
template<>
template<>
void BaseDecoder<MIX, ADIHandleBase>::SaveTo<RstTxt>(const std::string &outpath) {
  LOG(INFO) << "Save result data to:" << outpath;
  std::ofstream ofs{outpath, std::ios::binary};
  auto *praw = static_cast<RawDataDef *>(p_data);
  for (int i = 0; i < data_cnt; i++) {
	if (praw[i].type_ == DATA_TYPE_RST) {
	  ofs << fmt::format(
		  "{:4d} {:.5f} {:.12f} {:.12f} {:.5f} {:.3f} {:.3f} {:.3f} {:.3f} {:.3f} {:.3f} {:d} {:d}\n",
		  praw[i].gnss_.week,
		  praw[i].gpst,
		  praw[i].rst_.lat,
		  praw[i].rst_.lon,
		  praw[i].rst_.height,
		  praw[i].rst_.vn[0],
		  praw[i].rst_.vn[1],
		  praw[i].rst_.vn[2],
		  praw[i].rst_.atti[0],
		  praw[i].rst_.atti[1],
		  praw[i].rst_.atti[2],
		  praw[i].rst_.info.sensors,
		  praw[i].rst_.info.mode
	  );
	}
  }

}
template<>
template<>
void BaseDecoder<RST, RstHandle>::SaveTo<RstTxt>(const std::string &outpath) {
  LOG(INFO) << "Save result data to:" << outpath;
  std::ofstream ofs{outpath, std::ios::binary};
  auto *praw = static_cast<NavOutput *>(p_data);
  for (int i = 0; i < data_cnt; i++) {
	ofs << fmt::format(
		"{:4d} {:.5f} {:.12f} {:.12f} {:.5f} {:.3f} {:.3f} {:.3f} {:.3f} {:.3f} {:.3f} {:d} {:d}\n",
		praw[i].week,
		praw[i].gpst,
		praw[i].lat,
		praw[i].lon,
		praw[i].height,
		praw[i].vn[0],
		praw[i].vn[1],
		praw[i].vn[2],
		praw[i].atti[0],
		praw[i].atti[1],
		praw[i].atti[2],
		praw[i].info.sensors,
		praw[i].info.mode
	);
  }
}

template<RawFileType T1, class T>
BaseDecoder<T1, T>::~BaseDecoder() {
  delete (RawDataDef *)p_data;
}
template<RawFileType T1, class T>
BaseDecoder<T1, T>::BaseDecoder() {
  google::InitGoogleLogging(".");
  google::LogToStderr();
}
template<RawFileType T1, class H>
BaseDecoder<T1, H>::BaseDecoder(const H &handle) {
  this->handle = handle;
}

void RawDecoder::SaveToImu(const std::string &outpath) {
  SaveTo<ImuTxt>(outpath);
}

void RawDecoder::SaveToImd(const std::string &outpath) {
  SaveTo<Imd>(outpath);
}
void RawDecoder::SaveToGnss(const std::string &outpath) {
  SaveTo<GnssTxt>(outpath);
}
void RawDecoder::SaveToVel(const std::string &outpath) {
  SaveTo<VelTxt>(outpath);
}
RawDecoder::RawDecoder(const ADIHandleBase &handle) : BaseDecoder<MIX, ADIHandleBase>(handle) {
}
void RawDecoder::SaveToRst(const std::string &outpath) {
  SaveTo<RstTxt>(outpath);
}
void RawDecoder::SaveToBmp(const std::string &outpath) {
  SaveTo<BmpTxt>(outpath);
}
RawDecoder::~RawDecoder() = default;

CptDecoder::CptDecoder() : BaseDecoder<CPT, CptHandle>() {
}

/**
 * 加载CPT日志
 * @param raw_path
 */
template<>
void BaseDecoder<CPT, CptHandle>::Load(const std::string &raw_path) {

}

template<>
template<>
void BaseDecoder<CPT, CptHandle>::SaveTo<ImuTxt>(const std::string &outpath) {

}
void CptDecoder::SaveToImuTxt(const std::string &outpath) {
  SaveTo<ImuTxt>(outpath);
}
CptDecoder::~CptDecoder() = default;
RstDecoder::RstDecoder() : BaseDecoder<RST, RstHandle>() {

}
void RstDecoder::SaveToRstTxt(const std::string &outpath) {
  SaveTo<RstTxt>(outpath);
}
RstDecoder::~RstDecoder() = default;
}